package it.unibo.refolding.pico;

import it.unimi.dsi.fastutil.doubles.DoubleArraySet;
import it.unimi.dsi.fastutil.doubles.DoubleSet;

import java.awt.geom.CubicCurve2D;
import java.util.logging.Level;
import java.util.logging.Logger;

import org.apache.commons.lang3.ArrayUtils;

import com.google.common.base.Preconditions;

public class Wlc {

	public static final double KB = 1.3806505000000001e-23;
	
	private static final Logger logger = Logger.getLogger(Wlc.class.getSimpleName());

	public static double evaluateContourLengthPartialDerivative(double kbtOverP, double x, double lc) {
		double xOverLc = x / lc;
		double oneMinusXOverLc = 1 - xOverLc;
		return -kbtOverP / 2 * xOverLc / lc * (Math.pow(oneMinusXOverLc, -3) + 2);
	}

	public static double evaluate(double kbtOverP, double xOverLc) {
		double oneMinusXOverLc = 1 - xOverLc;
		return kbtOverP / 4 * (1 / (oneMinusXOverLc * oneMinusXOverLc) + 4 * xOverLc - 1);
	}

	public final double kbtOverP, k, lc;

	public Wlc(double kbtOverP, double k, double lc) {
		this.kbtOverP = kbtOverP;
		this.k = k;
		this.lc = lc;
	}

	public double computeExtension(double y) {
		return computeExtension(kbtOverP, k, lc, y);
	}

	public double evaluateForDistance(double x) {
		return evaluate(kbtOverP, x / lc);
	}

	public double computeEndToEndDistance(double y) {
		return computeEndToEndDistance(kbtOverP, lc, y);
	}

	/**
	 * 
	 * @param y
	 *            force
	 * @param lc
	 * @param k
	 * @return
	 */
	public static double computeExtension(double kbtOverP, double k, double lc, double y) {
		double k_lc = k * lc;
		double kb_t_over_p_over_k_lc = kbtOverP / k_lc;
		double twelve_kb_t_over_p_over_k_lc_plus_four_times_y = (12 * kb_t_over_p_over_k_lc + 4)
				* y;
		double six_kb_t_over_p_over_k_lc_plus_four_times_y = (6 * kb_t_over_p_over_k_lc + 4) * y;
		double y_square = y * y;

		double[] c = {
				-4 * kbtOverP,
				twelve_kb_t_over_p_over_k_lc_plus_four_times_y + 9 * kbtOverP,
				-((twelve_kb_t_over_p_over_k_lc_plus_four_times_y + 4 * y)
						/ k_lc
						* y
						+ (twelve_kb_t_over_p_over_k_lc_plus_four_times_y + six_kb_t_over_p_over_k_lc_plus_four_times_y) + 6 * kbtOverP),
				(4 * kb_t_over_p_over_k_lc + 4) / (k_lc * k_lc) * y_square * y
						+ (9 * kb_t_over_p_over_k_lc + 8) / k_lc * y_square
						+ six_kb_t_over_p_over_k_lc_plus_four_times_y,};
		ArrayUtils.reverse(c);
		double[] roots = new double[3];
		int rootCount = CubicCurve2D.solveCubic(c, roots);
		Preconditions.checkState(rootCount != -1, "Constant equation.");
		Preconditions.checkState(rootCount != 0, "No roots found at all.");
		DoubleSet acceptableRoots = new DoubleArraySet(rootCount);
		for (int i = 0; i < rootCount; ++i) {
			if (y / k_lc <= roots[i] && roots[i] <= 1 + y / k_lc) {
				acceptableRoots.add(roots[i]);
			}
		}
		// Seen it twice failing with refolding on mica in air.
		if (acceptableRoots.size() != 1) {
			logger.log(Level.SEVERE, "kbtOverP={1} k={2} lc={3} y={4} acceptableRoots={5}",
					new Object[] {kbtOverP, k, lc, y, acceptableRoots});
			return Double.NaN;
		} else {
			return acceptableRoots.iterator().nextDouble() * lc;
		}
	}

	public static double computeEndToEndDistance(double kbtOverP, double lc, double y) {
		return computeExtension(kbtOverP, Double.POSITIVE_INFINITY, lc, y);
	}

	/**
	 * Seee find_x_for_tug_of_war in wlc.py.
	 */
	public static double computeEndToEndDistanceForTugOfWar(
		double kbtOverP, double countourLength, double springConstantNPerM,
		double z) {
		throw new IllegalStateException("not yet implemented");
	}
}
